import copy
import cv2
from cv_bridge import CvBridge, CvBridgeError

import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import Bool,String
from acupoint_msgs.msg import AcupointImagePixel




RAD2DEG=180/3.1415926
DEG2RAD=3.1415926/180

class WorkingAcupointDisplay:
    def __init__(self):
        self.origin_image=None
        self.bridge=CvBridge()
        
        self.acupoint_sub = rospy.Subscriber("/acupoint/acu_filter_image_pixel", AcupointImagePixel, self.acupointCallback) 
        self.img_sub = rospy.Subscriber("/k4a/color/color", Image,self.imgCallback,queue_size=1)
        self.acupoint_display_pub=rospy.Subscriber("/working_acupoint",String,self.acupointNameCallback)
        self.x_lists=[]
        self.y_lists=[]
        self.jaka_arrive=False
        self.acupoint_name=None
        self.acupoint_pixels=None
        self.display_acupoint=None


        print("wait for image and acupoint")
        while (not rospy.is_shutdown()) and (self.origin_image is None or self.acupoint_name is None):
            rospy.sleep(0.1)
        
        print("display working acupoint")
        flag=0
        while not rospy.is_shutdown():
            for name,pixel in zip(self.acupoint_name,self.acupoint_pixels):
                y_plot = int(pixel.x)
                x_plot = int(pixel.y)
                cv2.circle(self.origin_image, (x_plot, y_plot), 4, (255, 0, 0), thickness=-1)
                
            if self.display_acupoint=="end":
                self.display_acupoint=None
            elif self.display_acupoint=="left_leg":
                index=self.acupoint_name.index("l_BL40")
                x1=self.acupoint_pixels[index].x
                y1=self.acupoint_pixels[index].y
                index=self.acupoint_name.index("l_BL59")
                x2=self.acupoint_pixels[index].x
                y2=self.acupoint_pixels[index].y
                
                y_plot = int((x1+x2)/2.0)
                x_plot = int((y1+y2)/2.0)
                cv2.circle(self.origin_image, (x_plot, y_plot), 8, (0, 255, 255), thickness=-1)   
                 
            elif self.display_acupoint is not None:
                index=self.acupoint_name.index(self.display_acupoint)
                x=self.acupoint_pixels[index].x
                y=self.acupoint_pixels[index].y
                y_plot = int(x)
                x_plot = int(y)
                cv2.circle(self.origin_image, (x_plot, y_plot), 8, (0, 255, 255), thickness=-1)   
            else:
                pass
            
            cv2.namedWindow("acupoint_display", 0)
            cv2.imshow("acupoint_display", self.origin_image)
            cv2.waitKey(100)
            rospy.sleep(0.1)
            
        
    def acupointNameCallback(self,msg):
        self.display_acupoint=msg.data   
        print(self.display_acupoint)
         
    def imgCallback(self,image_msg):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
            self.origin_image = cv_image.copy()
        except:
            pass
    
    def acupointCallback(self,msg):
        self.acupoint_name=copy.deepcopy(msg).name
        self.acupoint_pixels=copy.deepcopy(msg).pixels
     

if __name__ == "__main__":
    rospy.init_node('working_acupoint_display_node')
    wad=WorkingAcupointDisplay()



    

    
